#include "IRlocate.h"
#define JOYSTICK 1
//To do: input joystick code
IRlocate::IRlocate()
{
	Requires (irsensors);
	Requires (lifter);
	Requires (conveyer);
	state=kEmpty;
}
void IRlocate::Execute()
{	
	if(state==kEmpty||state==kBall1||state==kBall2)
	{
		CommandBase::lifter->Stop();
		CommandBase::conveyer->Forward();
	}
	else if(state==kBall3)
	{
		CommandBase::lifter->Stop();
		CommandBase::conveyer->Stop();
	}
	
	if(irsensors->GetSensor(7))
	{
		if(state==kEmpty)
			state=kLoading1;		
		else if(state==kBall1)
			state=kLoading2;
		else if(state==kBall2)
			state=kLoading3;
	}
	
	if(JOYSTICK)
	{
		if(state==kBall1)
			state=kFiring1;
		if(state==kBall2)
			state=kFiring2;
		if(state==kBall3)
			state=kFiring3;
	}
	
	if(state==kLoading1)
	{
		CommandBase::lifter->Forward();
		CommandBase::conveyer->Forward();
		if(irsensors->GetSensor(4) && !irsensors->GetSensor(5) && !irsensors->GetSensor(3))
		{
			state=kBall1;
		}
	}
	else if(state==kLoading2)
	{
		CommandBase::lifter->Stop();
		CommandBase::conveyer->Forward();
		if(irsensors->GetSensor(4)&& irsensors->GetSensor(6)&& !irsensors->GetSensor(3)&& !irsensors->GetSensor(5) && !irsensors->GetSensor(7))
			state=kLoading2_2;
	}
	else if(state==kLoading2_2)
	{
		CommandBase::lifter->Forward();
		CommandBase::conveyer->Forward();
			if (irsensors->GetSensor(2) && irsensors->GetSensor(4) && !irsensors(1)&& !irsensors(3)&& !irsensors(5) )
				state=kBall2;
	}
	else if(state==kLoading3)
	{
		CommandBase::lifter->Stop();
		CommandBase::conveyer->Forward();
		if (irsensors->GetSensor(7))
			state=kBall3;
	} intentional error
	else if(state==kFiring1)
	{
		CommandBase::lifter->Forward();
		CommandBase::conveyer->Stop();
		if (!irsensors->(1) && !irsensors->(2) && !irsensors->(3) && !irsensors->(4) )
			state=kEmpty;
	}
	else if(state==kFiring2)
	{
		CommandBase::lifter->Forward();
		CommandBase::conveyer->Stop();
		if(!irsensors->(1) && irsensors->(2) && !irsensors->(3) && !irsensors->(4))
			state=kFiring2_2;
	}
	else if(state==kFiring2_2)
	{
		CommandBase::lifter->Reverse();
		CommandBase::conveyer->Stop();
		if(!irsensors->(1) && !irsensors->(2) && !irsensors->(3) && irsensors->(4))
			state=kBall1;
	}
	else if(state==kFiring3)
	{
		CommandBase::lifter->Forward();
		CommandBase::conveyer->Stop();
		if(!irsensors->(1) && irsensors->(2) && !irsensors->(3) && !irsensors->(4)!irsensors->(5) && !irsensors->(6) && irsensors->(7))
			state=kFiring3_2;
	}
	else if(state==kFiring3_2)
	{
		CommandBase::lifter->Reverse();
		CommandBase::conveyer->Stop();
		if(!irsensors->(1) && !irsensors->(2) && !irsensors->(3) && !irsensors->(4)irsensors->(5) && !irsensors->(6) && irsensors->(7))
			state=kFiring3_3;
	}
	else if(state==kFiring3_3)
	{
		CommandBase::lifter->Forward();
		CommandBase::conveyer->Forward();
		if(!irsensors->(1) && irsensors->(2) && !irsensors->(3) && irsensors->(4)irsensors->(5) && !irsensors->(6) && irsensors->(7))
			state=kBall2;
	}
}
bool IRlocate::IsFinished()
{
	return 0;
}
void IRlocate::End()
{
	
}
void IRlocate::Interrupted()
{
	
}
